/**
 * Author(s): xuming.Wong <xuming.Wong@gmail.com>
 */
#include "RigidGameObject.h"
#include "RigidBodyComponent.h"

namespace PQEngine
{
	RigidGameObject::RigidGameObject(GameObject* object)
	{
		_gameObject=object;
		if(_gameObject==NULL){
			return;
		}

		initCollisionShape();
		initRigidBody();
	}

	void RigidGameObject::initCollisionShape()
	{
		_collisionShape=NULL;
		Component* component=_gameObject->getColliderComponent();

		if(Object::isCollider(component)){
			ObjectType type=component->getObjectType();
			switch(type){
				case OBJECT_TYPE_COMPONENT_PLANE_COLLIDER:
					initPlaneShape((PlaneColliderComponent*)component);
					break;
				case OBJECT_TYPE_COMPONENT_BOX_COLLIDER:
					initBoxShape((BoxColliderComponent*)component);
					break;
				case OBJECT_TYPE_COMPONENT_SPHERE_COLLIDER:
					initSphereShape((SphereColliderComponent*)component);
					break;
			}
		}else{
			//shouldn't be ,there is a bug

			assert("Bug: initCollisionShape");
		}
	}

	void RigidGameObject::initSphereShape(SphereColliderComponent* collider)
	{
		if(collider==NULL){
			return;
		}

		float radius=collider->getRadius();
		_collisionShape=new btSphereShape(radius);
	}

	void RigidGameObject::initPlaneShape(PlaneColliderComponent* collider)
	{
		if(collider==NULL){
			return;
		}

		Ogre::Vector3 normal=collider->getNormal();
		float distance=collider->getDistance();
		_collisionShape=new btStaticPlaneShape(MathUtil::toBullet(normal),distance);
	}

	void RigidGameObject::initBoxShape(BoxColliderComponent* collider)
	{
		if(collider==NULL){
			return;
		}

		Ogre::Vector3 size=collider->getSize();
		_collisionShape=new btBoxShape(MathUtil::toBullet(size/2));
		
	}

	void RigidGameObject::initRigidBody()
	{
		if(_collisionShape==NULL){
			return;
		}

		RigidBodyComponent* rigidBodyComponent=_gameObject->getRigidBodyComponent();
		if(rigidBodyComponent==NULL){
			return;
		}

		float mass=rigidBodyComponent->getMass();
		float restitution=rigidBodyComponent->getRestitution();
		float friction=rigidBodyComponent->getFriction();
		
		btVector3 localInertiaTensor = btVector3(0,0,0);
		if (mass > 0.0){
	        _collisionShape->calculateLocalInertia(mass, localInertiaTensor);
		}

		_objectState = new GameObjectState(_gameObject);
		_rigidBody = new btRigidBody(mass, _objectState, _collisionShape, localInertiaTensor);
        _rigidBody->setRestitution(restitution);
        _rigidBody->setFriction(friction);

		Ogre::Vector3 worldPosition=_gameObject->getWorldPosition();
		Ogre::Quaternion worldOrient=_gameObject->getWorldOrientation();

		btVector3 btPosition=MathUtil::toBullet(worldPosition);
		btQuaternion btRotation=MathUtil::toBullet(worldOrient);

		_rigidBody->getWorldTransform().setOrigin(btPosition);
        _rigidBody->getWorldTransform().setRotation(btRotation);

	}

	btRigidBody* RigidGameObject::getRigidBody()
	{
		return _rigidBody;
	}

	void RigidGameObject::setKinematicObject(bool isKinematic) 
	{
		if (this->isKinematicObject() != isKinematic) {
			_rigidBody->setCollisionFlags(_rigidBody->getCollisionFlags()^btCollisionObject::CF_KINEMATIC_OBJECT);
		}
	}

	bool RigidGameObject::isKinematicObject()
	{
		return _rigidBody->isKinematicObject();
	}

	void RigidGameObject::setLinearVelocity(const Ogre::Vector3 &v)
    {
        _rigidBody->setLinearVelocity(btVector3(v.x, v.y, v.z));
    }

	Ogre::Vector3 RigidGameObject::getLinearVelocity()
    {
		const btVector3 v = _rigidBody->getLinearVelocity();
		return MathUtil::toOgre(v);
    }

	void RigidGameObject::applyImpulse(const Ogre::Vector3 &impulse, const Ogre::Vector3 &position)
    {
        _rigidBody->applyImpulse(MathUtil::toBullet(impulse), MathUtil::toBullet(position));
    }

	void RigidGameObject::applyForce(const Ogre::Vector3 &impulse, const Ogre::Vector3 &position)
    {
		_rigidBody->applyForce(MathUtil::toBullet(impulse), MathUtil::toBullet(position));
    }

    Ogre::Vector3 RigidGameObject::getCenterOfMassPivot(const Ogre::Vector3 &pivotPosition) const
    {
		btVector3 v= _rigidBody->getCenterOfMassTransform().inverse()*MathUtil::toBullet(pivotPosition);
		return MathUtil::toOgre(v);
    }

    void RigidGameObject::setDeactivationTime(const float ftime)
    {
		_rigidBody->setDeactivationTime(ftime);
    }

	void RigidGameObject::setDamping(const float linearDamping, const float angularDamping)
    {
        _rigidBody->setDamping( linearDamping,  angularDamping);
    }

}
